//
//  erza_matrix_lib.h
//  KFilter
//
//  Created by Mita Yun on 6/20/13.
//  Copyright (c) 2013 com.mika. All rights reserved.
//

#ifndef KFilter_erza_matrix_lib_h
#define KFilter_erza_matrix_lib_h

#define BENCH_START 	for(int pp = 0; pp<10;pp++){clock_t begin, end;double time_spent;begin = clock();for (int i = 100000; i > 0; i--) {

#define BENCH_END		}end = clock();time_spent = (double) (end - begin) / CLOCKS_PER_SEC;printf("kmbench %f \r\n", time_spent);}

#define TRUE 1
#define FALSE 0

#include "arm_math.h"
#include <time.h>

int qrDecompositionT_f32(arm_matrix_instance_f32 *A, arm_matrix_instance_f32 *Q, arm_matrix_instance_f32 *R);
void matrixDiv_f32(arm_matrix_instance_f32 *X, arm_matrix_instance_f32 *A, arm_matrix_instance_f32 *B, arm_matrix_instance_f32 *Q, arm_matrix_instance_f32 *R, arm_matrix_instance_f32 *AQ);

void matrixInit(arm_matrix_instance_f32 *A, int rows, int cols);

void matrixFree(arm_matrix_instance_f32 *A);

arm_status qr(arm_matrix_instance_f32 *A, arm_matrix_instance_f32 *Q, arm_matrix_instance_f32 *R);

void matrixPrint(arm_matrix_instance_f32 *m);
void mprintf(char * name, arm_matrix_instance_f32 *A);
float32_t matrixGet(arm_matrix_instance_f32 *A, int rowIndex, int colIndex);

void matrixSet(float32_t data, arm_matrix_instance_f32 *A, int rowIndex, int colIndex);

float32_t vectorMag(float32_t *x, int start, int end);

void vectorNorm(float32_t *src, float32_t *dest, int start, int end);

void vectorCopy(float32_t *src, float32_t *dest, int start, int end);
void vectorPrint(float32_t *x, int start, int end);


void quatNorm(float *inQuat, float *outQuat);
void quatToEuler(float *quat, float *outYaw, float *outPitch, float *outRoll);

#endif
